#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <iostream>
#include <fstream>
#include <time.h>
#include <vector>

#include "Reseau.h"
#include "Robot.h"
#include "Outils.h"
#include "NormalDistribution.h"

const double SIGMA = 0.447;
const double NORM_VALUE = 0.01;


#define SIM

#ifdef SIM
#include <webots/Supervisor.hpp>
#endif

using namespace std;

struct itemRobot{
    Robot * monRobot;
    itemRobot* pPrecedent;
    itemRobot* pSuivant;
};

int rdtsc()
{
    __asm__ __volatile__("rdtsc");
}


vector<Robot> generation;

itemRobot * TrierFitness(Robot ** lesRobots);

int main()
{
  Robot * lesRobots[POPULATION];
  int indice;


  debug("Création de la population d'équipes");
  //Limite entre les deux demi terrains
  double coordLimite[2];
  //coordonnées du bord du terrain
  double coordBordsTerrain[4][2];
  double positionRobot;
  double positionBalle;
  int nbButs;
  int parent;
  int indice1;
  int indice2;
  structInfosSortie infosSortie[NB_JEUX];
  NormalDistribution normalDistrib = NormalDistribution ( SIGMA, 0, NORM_VALUE );



  //Initialiser coordLimite et coordBordsTerrains
  #ifdef SIM
debug("calcul coord terrain");

webots::Supervisor master;
webots::Field* coordKhep1 = master.getFromDef("Khep1")->getField("translation");
webots::Field* rotKhep1 = master.getFromDef("Khep1")->getField("rotation");
webots::Field* coordKhep2 = master.getFromDef("Khep2")->getField("translation");
webots::Field* rotKhep2 = master.getFromDef("Khep2")->getField("rotation");
webots::Field* coordBall = master.getFromDef("Ball")->getField("translation");
webots::Node * terrain = master.getFromDef("Terrain");
webots::Field * solTerrain = master.getFromDef("SolTerrain")->getField("translation");
double radiusBall=0.04;
double decalTerrainX=solTerrain->getSFVec3f()[0];
double decalTerrainZ=solTerrain->getSFVec3f()[2];
//coordBordsTerrain[numeroDuPoint][0=x,1=Y(ouZ)]
//numerodespoint:1 basdroite 2 basgauche 3 hautguahce 4 hautdroite
debug("calcul coord terrain");
coordBordsTerrain[0][0]=decalTerrainX;
coordBordsTerrain[0][1]=decalTerrainZ;
coordBordsTerrain[1][0]=(terrain->getField("xDimension")->getSFInt32()-1)*terrain->getField("xSpacing")->getSFFloat()+decalTerrainX;
coordBordsTerrain[1][1]=decalTerrainZ;
coordBordsTerrain[3][0]=decalTerrainX;
coordBordsTerrain[3][1]=decalTerrainZ+(terrain->getField("zDimension")->getSFInt32()-1)*terrain->getField("zSpacing")->getSFFloat();
coordBordsTerrain[2][0]=(terrain->getField("xDimension")->getSFInt32()-1)*terrain->getField("xSpacing")->getSFFloat()+decalTerrainX;
coordBordsTerrain[2][1]=decalTerrainZ+(terrain->getField("zDimension")->getSFInt32()-1)*terrain->getField("zSpacing")->getSFFloat();
debug("calcul coord terrain");

  #endif

  //Pour début... il faudra ensuite loader les poids depuis fichier
 for(int i = 0 ; i < POPULATION ; i++)
 {
   
    lesRobots[i] = new Robot();
 }

  for(int nbGeneration = 0; nbGeneration < NB_GEN ; nbGeneration++)
  {
    debug("lancement du test");
    for(int i = 0 ; i < POPULATION ; i++)
    {
	for(int j = 0 ; j = NB_JEUX ; j++)
	{
	  //On doit les faire jouer plusieurs tirs... Genre quelques dizaines voire une centaine.
	  #ifdef SIM
const double rot[4]={0,1,0,3.14};
	const double values1[3]={coordBordsTerrain[0][0]+(coordBordsTerrain[1][0]-coordBordsTerrain[0][0])/4,0,coordBordsTerrain[0][1]+(coordBordsTerrain[4][1]-coordBordsTerrain[0][1])/8};
	coordKhep1->setSFVec3f(values1);
rotKhep1->setSFRotation(rot);
	const double values2[3]={coordBordsTerrain[0][0]+(coordBordsTerrain[1][0]-coordBordsTerrain[0][0])*3/4,0,coordBordsTerrain[0][1]+(coordBordsTerrain[4][1]-coordBordsTerrain[0][1])/8};	
	coordKhep2->setSFVec3f(values2);
rotKhep2->setSFRotation(rot);
const double valuesB[3]={coordBordsTerrain[0][0]+(coordBordsTerrain[1][0]-coordBordsTerrain[0][0])/2,0.0395792,coordBordsTerrain[0][1]+(coordBordsTerrain[4][1]-coordBordsTerrain[0][1])/4};
coordBall->setSFVec3f(valuesB);
#endif
      infosSortie[j]=lesRobots[i]->Jouer(&master,coordKhep1,coordKhep2,coordBall,coordBordsTerrain,radiusBall);

	}
	lesRobots[i]->CalculerFitness(infosSortie);  
        lesRobots[i]->Save("poidsAttaquants.txt");
    }

    
    debug("Tri des robots");
    //Tri des robots en fonction de leur fitness
    itemRobot * premier =  TrierFitness(lesRobots);
    itemRobot * elem = premier;

    //Reproduction
    debug("Reproduction et mutation des robots");
    //On sauvegarde l'ancienne génération, tout en "biaisant " le hasard : on met plus d'individus dont la fitness en grande !
    int i=0;
    while(elem != NULL)
    {
      for(int j = 0; j<POPULATION-i ; j++)
      {
	generation.push_back(*(elem->monRobot));
      }
      i++;
	elem=elem->pSuivant;
    }
    elem = premier;
    while(elem != NULL)
    {
      // on tire au sort les deux parents
      srand(rdtsc());
      indice1 = rand()%(generation.size());
      indice2 = rand()%(generation.size());



      //on regroupe les matrices pour former le nouvel individu
      //poidsEntreeInter
      for (int j = 0; j < NEURONES_ENTREE ; j++)
      {
	for(int k = 0 ; k < NEURONES_INTER ; k++)
	{
	  //On selectionne au hasard le gène du parent1 ou du parent2
	  parent = (rand () % 2) == 0 ? indice1 : indice2;

	  //on donne au réseau les nouvelles valeurs avec la mutation
	  (elem->monRobot->GetReseau())->ModifierPoids1(i, j, generation[parent].GetReseau()->GetPoids1(i, j));
	}
      }


      //poidsInterSortie
      for (int j = 0; j < NEURONES_INTER ; j++)
      {
	for(int k = 0 ; k < NEURONES_SORTIE ; k++)
	{
	  //On selectionne au hasard le gène du parent1 ou du parent2
	  parent = (rand () % 2) == 0 ? indice1 : indice2;

	  //on donne au réseau les nouvelles valeurs avec la mutation
	  (elem->monRobot->GetReseau())->ModifierPoids2(i, j, generation[parent].GetReseau()->GetPoids2(i, j));
	}
      }

      //on réinitialise la fitness
      elem->monRobot->SetFitness(0);

      elem=elem->pSuivant;
    }
  }
  return 0;
}


itemRobot* TrierFitness(Robot ** lesRobots)
{
  itemRobot * elem = new itemRobot();
  itemRobot * premier = new itemRobot();

  elem->monRobot = lesRobots[0];
  elem->monRobot->SetFitness(lesRobots[0]->GetFitness());
  elem->pPrecedent = NULL;
  elem->pSuivant = NULL;

  premier = elem;


 for(int i = 1 ; i < POPULATION ; i++)
 {
   while(elem->pSuivant != NULL && elem->monRobot->GetFitness() > lesRobots[i]->GetFitness())
   {
     elem = elem->pSuivant;
   }
   if(elem->monRobot->GetFitness() <= lesRobots[i]->GetFitness())
   {
     itemRobot* newRobot = new itemRobot();
     newRobot->monRobot = lesRobots[i];
     newRobot->monRobot->SetFitness(lesRobots[i]->GetFitness());
     newRobot->pPrecedent = elem->pPrecedent;
     newRobot->pSuivant = elem;
     elem->pPrecedent = newRobot;
     if(newRobot->pPrecedent == NULL)
     {
       premier = newRobot;
     }
     #ifdef DEBUG_EXT
     cout << "Element : " << elem << " Fitness : " << elem->monRobot->GetFitness() << " Précédent : " << elem->pPrecedent << " Suivant : " << elem->pSuivant << endl;
     cout << "Nouveau : " << newRobot << " Fitness : " << newRobot->monRobot->GetFitness() << " Précédent : " << newRobot->pPrecedent << " Suivant : " << newRobot->pSuivant << endl;
     #endif
   }
   else
   {
     itemRobot* newRobot = new itemRobot();
     newRobot->monRobot = lesRobots[i];
     newRobot->monRobot->SetFitness(lesRobots[i]->GetFitness());
     newRobot->pPrecedent = elem;
     newRobot->pSuivant = NULL;
     elem->pSuivant = newRobot;

     if(newRobot->pPrecedent == NULL)
     {
       premier = newRobot;
     }

     #ifdef DEBUG_EXT
     cout << "Element : " << elem << " Fitness : " << elem->monRobot->GetFitness() << " Précédent : " << elem->pPrecedent << " Suivant : " << elem->pSuivant << endl;
     cout << "Nouveau : " << newRobot << " Fitness : " << newRobot->monRobot->GetFitness() << " Précédent : " << newRobot->pPrecedent << " Suivant : " << newRobot->pSuivant << endl;
     #endif

   }
   elem=premier;
 }

 return elem;
}
